﻿#include "ut_coordinates.h"
#include "ut_algorithm.h"
#include "ut_math.h"
#include <math.h>

BEGIN_SPACE_LANGE_VDR

#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288
#endif // !M_PI

/**
 * @brief 将当前瓦片数据转为uint64_t数值
 * @return 返回转换后的数值
 */
// UTCoordinates::TileID UTCoordinates::TileIDInfo::toTileID() 
// {
//     TileID tileID = 0;
//     tileID |= level;
//     tileID <<= 28;
//     tileID |= anchor.y;
//     tileID <<= 28;
//     tileID |= anchor.x;
//     return tileID;
// }

/**
 * @brief 通知指定的数值，为当前结构赋值
 * @param val - 数值
 */
// void UTCoordinates::TileIDInfo::fromTileID(const TileID & val)
// {
//     anchor.x = val&0xFFFFFFF;
//     anchor.y = val>>28&0xFFFFFFF;
//     level = val>>56;
// }

/**
 * @brief 计算指定级别下瓦片尺寸（单位：像素）
 * @param level 级别
 * @return 返回计算的瓦片尺寸（单位：像素）
 */
uint64_t UTCoordinates::TileSize(const uint16_t level)
{
    const uint64_t val=256;
    return val<<level;
}

/**
 * @brief  指定级别下，赤道处瓦片分辨率计算（单位：米）
 * @param  level   级别 
 * @return 返回瓦片分辨率（单位像素尺寸 单位：米）
 */
double UTCoordinates::TileResolution(const int32_t level)
{
    return INITIAL_RESOLUTION/static_cast<double>(1<<level);
}

/**
 * @brief 将指定级别下的瓦片像素坐标转为投影坐标
 * @param x        x瓦片像素
 * @param y        y瓦片像素
 * @param level   级别
 * @param proj    投影系
 * @return 返回转换后的投影坐标
 */
UTCoordinates::ProjPoint UTCoordinates::TilePixelToProj(const double x,const double y,
    const int32_t level,const ProjType proj/*=ProjType::PROJ_TYPE_EPSG_900913*/)
{
    const double resol=TileResolution(level);
    ProjPoint projPoint(x*resol-ORIGIN_SHIFT,y*resol-ORIGIN_SHIFT);
    if(ProjType::PROJ_TYPE_EPSG_900913==proj)
    {
        return projPoint;
    }
    else
    {
        return ProjToLonLat(projPoint.GetX(),projPoint.GetY(),
            ProjType::PROJ_TYPE_EPSG_900913); 
    }
}

/**
 * @brief 将指定级别下的投影坐标转为瓦片像素坐标
 * @param x        x坐标
 * @param y        y坐标
 * @param level   级别
 * @param proj    投影系
 * @return 返回转换后的瓦片像素坐标
 */
UTCoordinates::PixelPoint UTCoordinates::ProjToTilePixel(const double x,const double y,
    const int32_t level,const ProjType proj/*=ProjType::PROJ_TYPE_EPSG_900913*/)
{
    ProjPoint projPoint(x,y);
    if (ProjType::PROJ_TYPE_EPSG_900913!=proj)
    {
        projPoint=LonLatToProj(x,y,ProjType::PROJ_TYPE_EPSG_900913);
    }
    const double resol=TileResolution(level);
    return PixelPoint(static_cast<int32_t>(ceil((projPoint.GetX()+ORIGIN_SHIFT)/resol)),
        static_cast<int32_t>(ceil((projPoint.GetY()+ORIGIN_SHIFT)/resol)));
}

/**
 * @brief 瓦片像素转瓦片标识
 * @param x   x坐标
 * @param y   y坐标
 * @return 返回转换后的瓦片标识
 */
UTCoordinates::TileAnchor UTCoordinates::TilePixelToTileAnchor(const double x,const double y)
{
    TileAnchor anchor;
    anchor.x = static_cast<int>(ceil(x/256.0)-1.0);
    anchor.y = static_cast<int>(ceil(y/256.0)-1.0);
    return anchor;
}

/**
 * @brief 将瓦片标识转为瓦片像素
 * @param x   瓦片x标识
 * @param y   瓦片y标识
 * @return 返回转换后的瓦片像素
 */
UTCoordinates::PixelPoint UTCoordinates::TileAnchorToTilePixel(const int32_t x,const int32_t y)
{
    return PixelPoint(x*256,y*256);
}

/**
 * @brief 将经纬度转为投影坐标
 * @param lon   经度
 * @param lat   纬度
 * @param proj  投影系
 * @return 返回转换后的投影坐标
 */
UTCoordinates::ProjPoint UTCoordinates::LonLatToProj(const double lon,const double lat,
    const ProjType proj/*=ProjType::PROJ_TYPE_EPSG_900913*/)
{
    if(ProjType::PROJ_TYPE_EPSG_4326==proj||
       ProjType::PROJ_TYPE_LOCAL==proj)
    {
        return LonLatPoint(lon,lat);
    }
    else
    {
        double x=lon*ORIGIN_SHIFT/180.0;
        double y=log(tan((90.0+lat)*M_PI/360.0))/(M_PI/180.0);
        y=y*ORIGIN_SHIFT/180.0;
        return ProjPoint(x,y);
    }
}

/**
 * @brief 将投影坐标转换为经纬度坐标
 * @param x      x坐标
 * @param y      y坐标
 * @param proj   投影系
 * @return 返回转换后的经纬度坐标
 */
UTCoordinates::LonLatPoint UTCoordinates::ProjToLonLat(const double x,const double y,
    const ProjType proj/*=ProjType::PROJ_TYPE_EPSG_900913*/)
{
    if (ProjType::PROJ_TYPE_EPSG_4326==proj||
        ProjType::PROJ_TYPE_LOCAL==proj)
    {
        return LonLatPoint(x,y);
    }
    else
    {
        double lon=x/ORIGIN_SHIFT*180.0;
        double lat=y/ORIGIN_SHIFT*180.0;
        lat=180.0/M_PI*(2.0*atan(exp(lat*M_PI/180.0))-M_PI/2.0);
        return LonLatPoint(lon,lat);
    }
}
double UTCoordinates::DistanceProj(const ProjPoint &a,const ProjPoint &b)
{
    ProjPoint aProjPoint=LonLatToProj(a.GetX(),a.GetY());
    ProjPoint bProjPoint=LonLatToProj(b.GetX(),b.GetY());
    return vdr::UTAlgorithm::CalculateDistance(aProjPoint.GetX(),
        aProjPoint.GetY(),bProjPoint.GetX(),bProjPoint.GetY());
}
/**
 * @brief 计算两个点之间的距离
 * @param a - a点经纬度
 * @param b - b点经纬度
 * @return 返回两点之间的距离
 */
double UTCoordinates::DistanceMeters(const LonLatPoint &a,const LonLatPoint &b)
{
#define RAD(m) (m*M_PI/180.0)
#define HAVERSIN(m) (pow(sin(m/2), 2))
   	double radLat1 = RAD(a.GetY());
    double radLat2 = RAD(b.GetY());
    double sublat = radLat1 - radLat2;
    double sublon = RAD(a.GetX()) - RAD(b.GetX());
    double h = HAVERSIN(sublat) + cos(radLat1) * cos(radLat2) * HAVERSIN(sublon);
	double distance = 2 * EARTH_RADIUS * asin(sqrt(h));
	distance = (distance * 1000);
    return distance;
}

/**
 * @brief 获取指定范围内的全部瓦片
 * @param s - 起始位置投影坐标
 * @param e - 结束位置投影坐标 
 * @param level - 级别
 * @return 返回范围内的瓦片 
 */
std::vector<TileID> UTCoordinates::GetTiles(const ProjPoint & s,
                                                            const ProjPoint & e, 
                                                            const int32_t level)
{
    std::vector<TileID> tiles;
    PixelPoint px = ProjToTilePixel(s.GetX(), s.GetY(), level);
    TileAnchor sTile = TilePixelToTileAnchor(px.GetX(), px.GetY());
    px = ProjToTilePixel(e.GetX(), e.GetY(), level);
    TileAnchor eTile = TilePixelToTileAnchor(px.GetX(), px.GetY());
    for(uint32_t i=sTile.x;i<=eTile.x;++i) {
        for(uint32_t j=sTile.y;j<=eTile.y;++j) {
            tiles.push_back(UTCoordinates::MakeTileID(i,j,level));
        }
    }
    return tiles;
}

/**
 * @brief 将屏幕坐标(象素)转为投影坐标(米)
 * @param Point                屏幕坐标（单位：像素）
 * @param CenterProLonLat      中心点的投影坐标
 * @param ScreenHotXY          热点(地图旋转中心点)
 * @param fRes                 当前级别的分辨率
 * @param MaxExtent            全球范围-180 180 -90 90的投影坐标
 * @param fAngle               地图旋转角度
 * @param proj                 投影系(EPSG:900913, EPSG:4326, location(本地坐标,比如北京54和西安80等))
 * @return 投影坐标 
 */
MD2DPOINT UTCoordinates::ScreenToProj(const MF2DPOINT &Point,const MD2DPOINT &CenterProLonLat,
    const MI2DPOINT &ScreenHotXY,const float fRes, const MFUTRECT &MaxExtent,const float fAngle,
    const ProjType proj/*=ProjType::PROJ_TYPE_EPSG_900913*/)
{
    float fSinAngle=(float)UTMath::SinTimes1024((int)fAngle);
	float fCosAngle=(float)UTMath::CosTimes1024((int)fAngle);

	float x=Point.GetX()-ScreenHotXY.GetX();
	float y=Point.GetY()-ScreenHotXY.GetY();

	float fDelta_x=(float)(x*fCosAngle-y*fSinAngle);
	float fDelta_y=(float)(x*fSinAngle+y*fCosAngle);

	MD2DPOINT Result;

	Result.SetX(CenterProLonLat.GetX()+fDelta_x*fRes);
	Result.SetY(CenterProLonLat.GetY()-fDelta_y*fRes);

	if(ProjType::PROJ_TYPE_LOCAL!=proj)
	{
		while(Result.GetX()<MaxExtent.GetMinX())
		{
			Result.SetX(Result.GetX()+(MaxExtent.GetMaxX()-MaxExtent.GetMinX()));
		}
		while(Result.GetX()>MaxExtent.GetMaxX()) 
		{
			Result.SetX(Result.GetX()-(MaxExtent.GetMaxX()-MaxExtent.GetMinX()));
		}
		while(Result.GetY()<MaxExtent.GetMinY()) 
		{
			Result.SetY(Result.GetY()+(MaxExtent.GetMaxY()-MaxExtent.GetMinY()));
		}
		while(Result.GetY()>MaxExtent.GetMaxY()) 
		{
			Result.SetY(Result.GetY()-(MaxExtent.GetMaxY()-MaxExtent.GetMinY()));
		}
	}

	return Result;
}

/**
 * @brief 将投影坐标(米)转为屏幕坐标(象素)
 * @param ProLonLat            托投影坐标
 * @param CenterProLonLat      中心点的投影坐标
 * @param ScreenHotXY          热点(地图旋转中心点)
 * @param fRes                 当前级别的分辨率
 * @param fAngle               地图旋转角度
 * @return 屏幕坐标 
 */
MF2DPOINT UTCoordinates::ProjToScreen(const MD2DPOINT &ProLonLat,const MD2DPOINT &CenterProLonLat,
    const MI2DPOINT &ScreenHotXY,const float fRes,const float fAngle)
{
    MF2DPOINT Point;

	float x=(float)(ProLonLat.GetX()-CenterProLonLat.GetX());
	float y=(float)(ProLonLat.GetY()-CenterProLonLat.GetY());

	float fSinAngle=(float)UTMath::SinTimes1024((int32_t)fAngle);
	float fCosAngle=(float)UTMath::CosTimes1024((int32_t)fAngle);

	float fDelta_x=(float)(x*fCosAngle-y*fSinAngle);
	float fDelta_y=(float)(x*fSinAngle+y*fCosAngle);

	Point.SetX((float)(fDelta_x/fRes+ScreenHotXY.GetX()));
	Point.SetY((float)(ScreenHotXY.GetY()-fDelta_y/fRes));

	return Point;
}

/**
 * @brief 将屏幕坐标(象素)转为球面坐标(经纬度)
 * @param Point                屏幕坐标
 * @param CenterProLonLat      中心点的投影坐标
 * @param ScreenHotXY          热点(地图旋转中心点)
 * @param fRes                 当前级别的分辨率
 * @param MaxExtent            全球范围-180 180 -90 90的投影坐标
 * @param fAngle               地图旋转角度
 * @param proj                 投影系(EPSG:900913, EPSG:4326, location(本地坐标,比如北京54和西安80等))
 * @return 球面坐标 
 */
MF2DPOINT UTCoordinates::ScreenToLonLat(const MF2DPOINT &Point,const MD2DPOINT &CenterProLonLat,
    const MI2DPOINT &ScreenHotXY,const float fRes,const MFUTRECT &MaxExtent,const float fAngle,
    const ProjType proj/*=ProjType::PROJ_TYPE_EPSG_900913*/)
{
    MD2DPOINT point=ScreenToProj(Point,CenterProLonLat,ScreenHotXY,fRes,MaxExtent,fAngle,proj);
	point=ProjToLonLat(point.GetX(),point.GetY(),proj);
    return MF2DPOINT((float)point.GetX(),(float)point.GetY());
}

/**
 * @brief 将球面坐标(经纬度)转为屏幕坐标(象素)
 * @param LonLat                 球面坐标
 * @param CenterProLonLat        中心点的投影坐标
 * @param ScreenHotXY            热点(地图旋转中心点)
 * @param fRes                   当前级别的分辨率
 * @param fAngle                 地图旋转角度
 * @param proj                   投影系(EPSG:900913, EPSG:4326, location(本地坐标,比如北京54和西安80等))
 * @return 屏幕坐标 
 */
MF2DPOINT UTCoordinates::LonLatToScreen(const MD2DPOINT &LonLat,const MD2DPOINT &CenterProLonLat,
    const MI2DPOINT &ScreenHotXY,const float fRes,const float fAngle,
    const ProjType proj/*=ProjType::PROJ_TYPE_EPSG_900913*/)
{
    return ProjToScreen(LonLatToProj(LonLat.GetX(),LonLat.GetY(),proj),
        CenterProLonLat,ScreenHotXY,fRes,fAngle);
}

END_SPACE_LANGE_VDR
